/**
 *             _..--""-.                  .-""--.._
           _.-'         \ __...----...__ /         '-._
         .'      .:::...,'              ',...:::.      '.
        (     .'``'''::;                  ;::'''``'.     )
         \             '-)              (-'             /
          \             /                \             /
           \          .'.-.            .-.'.          /
            \         | \0|            |0/ |         /
            |          \  |   .-==-.   |  /          |
             \          `/`;          ;`\`          /
              '.._      (_ |  .-==-.  | _)      _..'
                  `"`"-`/ `/'        '\` \`-"`"`
                       / /`;   .==.   ;`\ \
                 .---./_/   \  .==.  /   \ \
                / '.    `-.__)       |    `"
               | =(`-.        '==.   ;
                \  '. `-.           /
                 \_:_)   `"--.....-'

                 file: gripper_node.cpp
                 description: node handle to determine when to close the gripper and with how much force.
                 authors: Michiel and Peter
 */

#include "gripper/gripper_node.h"

/**
 * Open or close motor.
 * @param	Int32	OPEN, CLOSED.
 */
void GripperNode::motorCallback(const std_msgs::Int32::ConstPtr &state)
{
	state_ = state->data;
}

/**
 * Init motor, subscribe to sensor.
 */
void GripperNode::init()
{
	// Create current publisher.
	current_pub_ = nh_.advertise<std_msgs::Float32>(Topic::GRIPPER_CURRENT, 1000);

	// Subscribe to motor callback
	motor_sub_ = nh_.subscribe(Topic::MOTOR, 1, &GripperNode::motorCallback, this);
	motor_.init();
}

/**
 * Publish current of the motor.
 */
void GripperNode::publishCurrent()
{
	C3mxlROS *interface = motor_.getInterfaceMotor();
	interface->getState();
	double current = interface->presentCurrent();

	std_msgs::Float32 msg;
	msg.data = current;
	current_pub_.publish(msg);
	ROS_INFO("Motor current: %fA", current);
}

/**
 * Publish current data and control the current.
 */
void GripperNode::spin()
{
	ros::Rate loop_rate(10);	// hertz

	int dir = GripperDefines::OPEN;
	int current_dir = dir;
	C3mxlROS *interface = motor_.getInterfaceMotor();

	while(ros::ok())
	{
		if(state_ == GripperDefines::CLOSED)
		{
			dir = GripperDefines::CLOSED;	// dicht
			ROS_INFO("Gripper is closed.");
		}
		else
		{
			dir = GripperDefines::OPEN;	// open
			ROS_INFO("Gripper is open.");
		}

		if(current_dir != dir)
		{
			timestamp_ = ros::Time::now() + ros::Duration(GripperDefines::GRIPPER_DURATION/1000);
			interface->setTorque(dir*(GripperDefines::DEFAULT_TORQUE/1000));
			current_dir = dir;
		}

		if(timestamp_ < ros::Time::now())
		{
			if(current_dir == GripperDefines::OPEN)
				interface->setTorque(0);
		}

		publishCurrent();	// publish current of motor

		loop_rate.sleep();
		ros::spinOnce();
	}
}

/**
 * Main.
 */
int main(int argc, char **argv)
{
	ros::init(argc, argv, "gripper_node");
	GripperNode motor_node;

	motor_node.init();
	motor_node.spin();

	return 0;
}
